#include "vehicle_linear_lv.h"

using namespace auto_ros::control;
//VehicleLinearLv(double mass, double cf, double cr, double lf, double lr, double ix,
//double iz, double h, double k_phi, double d_phi, double max_lateral_acc = 9, double max_tire_f_angle = 0.5);
int main()
{
	VehicleLinearLv test_vehicle_lv(1600, -110000, -92000, 1.12, 1.48, 700.7, 2059, 0.68, 145330, 4500);

	test_vehicle_lv.speed_update_matrix(10.0);

	std::cout << "state matrix is:" << std::endl
			  << test_vehicle_lv.state_matrix() << std::endl;
	std::cout << "control matrix is:" << std::endl
			  << test_vehicle_lv.control_matrix() << std::endl;
	std::cout << "distur matrix is:" << std::endl
			  << test_vehicle_lv.distur_matrix() << std::endl;
	std::cout << "out state matrix is:" << std::endl
			  << test_vehicle_lv.out_state_matrix() << std::endl;
	std::cout << "out distur matrix is:" << std::endl
			  << test_vehicle_lv.out_distur_matrix() << std::endl;
	std::shared_ptr<DiscreteLinearSys> test_lv_dis_ptr = test_vehicle_lv.discretization(0.01, "tustin");
	std::cout << "dis state matrix is:" << std::endl
			  << test_lv_dis_ptr->state_matrix() << std::endl;
	std::cout << "dis control matrix is:" << std::endl
			  << test_lv_dis_ptr->control_matrix() << std::endl;
	std::cout << "dis distur matrix is:" << std::endl
			  << test_lv_dis_ptr->distur_matrix() << std::endl;
	std::cout << "dis out state matrix is:" << std::endl
			  << test_lv_dis_ptr->out_state_matrix() << std::endl;
	std::cout << "dis out distur matrix is:" << std::endl
			  << test_lv_dis_ptr->out_distur_matrix() << std::endl;
	/*	bool calc_steady_state_ctr(
		const Eigen::VectorXd &distur_ref, const Eigen::VectorXd &y_controlled,
		Eigen::VectorXd &steady_state, Eigen::VectorXd &steady_control);*/
	Eigen::Vector2d distur_ref;
	Eigen::VectorXd y_controlled_ref(test_lv_dis_ptr->out_dim());
	Eigen::VectorXd steady_state(test_lv_dis_ptr->state_dim());
	Eigen::VectorXd steady_control(test_lv_dis_ptr->control_dim());
	distur_ref << 0.0, 0.01;
	y_controlled_ref << 0.0;
	test_lv_dis_ptr->calc_steady_state_ctr(distur_ref, y_controlled_ref, steady_state, steady_control);
	std::cout << "steady_state is:" << std::endl
			  << steady_state << std::endl;
	std::cout << "steady_control is:" << std::endl
			  << steady_control << std::endl;
}